function [ ind1, ind2, vEqual ] = findeqelem( v1, v2, tol ) %#codegen
%FINDEQELEM 找出向量v1与v2中相等的元素及序号
%
% Input Arguments:
% v1, v2: 向量，应同为单调递增或单调递减的
% tol: 认为v1与v2元素相等的阈值，默认为eps
%
% Output Arguments:
% ind1: 行向量，长度为v1及v2中相等元素的个数，v1的第ind1(i)个元素与v2的第ind2(i)个元素相等
% ind2: 行向量，长度与ind1相等
% vEqual: 列向量，长度与ind1相等，为v1及v2中相等的元素

if nargin < 3
    tol = eps;
end

if ~issorted(v1) || ~issorted(v2) || length(unique(v1))~=length(v1) || length(unique(v2))~=length(v2)
    error('findeqelem:notsortedorunique', 'Input vector(s) is not sorted or contains repeated element(s).');
end

if length(v1) > length(v2)
    [ind1, ind2, vEqual] = findeqelem_ls(v1, v2, tol);
else
    [ind2, ind1, vEqual] = findeqelem_ls(v2, v1, tol);
end
end

function [indLong, indShort, vEqual] = findeqelem_ls(vLong, vShort, tol) %#codegen
% NOTE: 本函数使用线性索引
j = 1; % 长向量序号
k = 1; % 相同向量序号
lenLong = length(vLong);
lenShort = length(vShort);
indLong = NaN(1, lenShort);
indShort = NaN(1, lenShort);
vEqual = NaN(lenShort, 1);
for i=1:lenShort % 短向量序号
    while ((j<=lenLong) && (vShort(i)>vLong(j)+tol))
        j = j + 1;
    end
    if j == (lenLong+1)
        break;
    end
    if (vShort(i)<=vLong(j)+tol) && (vShort(i)>=vLong(j)-tol)
        indLong(k) = j;
        indShort(k) = i;
        vEqual(k) = vShort(i);
        k = k + 1;
    end
end
indLong = indLong(1:k-1);
indShort = indShort(1:k-1);
vEqual = vEqual(1:k-1);
end